#include "ViewPoint.hpp"

ViewPoint::ViewPoint()
{
   mForward.set(1,0,0);
   mRight.set(0,1,0);
   mUp.set(0,0,1);
   UpdateMatrix();
}

void ViewPoint::SetOrigin(Vec3 aOrigin)
{
   mOrigin = aOrigin;
   UpdateMatrix();
}

void ViewPoint::PointAt(Vec3 aTarget,
                        Vec3 aRight,
                        Vec3 aUp)
{
   mForward = (aTarget - mOrigin).normal();
   mRight = cross(mForward, aUp).normal();
   mUp = cross(mRight, mForward).normal();
   mForward = cross(mUp, mRight);
   UpdateMatrix();
}

void ViewPoint::RotateAround(float aPivotDistance,
                             float aUpRotationAngle,
                             float aRightRotationAngle)
{
   Vec3 pivot = mOrigin + mForward * aPivotDistance;
   Vec3 origin2 = mOrigin - pivot;
   Mat4 rotationMatrix = math::rotation(aUpRotationAngle, mRight) 
                       * math::rotation(aRightRotationAngle, mUp);

   mOrigin = origin2 * rotationMatrix + pivot;
   mForward = mForward ^ rotationMatrix;
   mUp = mUp ^ rotationMatrix;
   mRight = mRight ^ rotationMatrix;
   UpdateMatrix();
}

void ViewPoint::UpdateMatrix()
{
   mWorldToLocal = 
      math::translate4(-mOrigin) * 
      Mat4(
      mRight[0],    mRight[1],    mRight[2], 0,
      mUp[0],    mUp[1],    mUp[2], 0,
      -mForward[0],   -mForward[1],  -mForward[2], 0,
      0,       0,       0,    1
      ).transpose();

   mLocalToWorld = mWorldToLocal.inverse();
}
